#ifndef PURE_PURSUIT_CONTROLLER_HPP_
#define PURE_PURSUIT_CONTROLLER_HPP_

#include <tf2_ros/buffer.h>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/srv/wait_and_insert_pose.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/utils.h"
#include <nav_msgs/msg/path.hpp>
#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"

class PurePursuitControllerD
{
public:
    PurePursuitControllerD(rclcpp::Node::SharedPtr &node, std::shared_ptr<tf2_ros::Buffer> tf);

    ~PurePursuitControllerD() {};

    bool insertPose(double len, geometry_msgs::msg::PoseStamped pose, nav_msgs::msg::Path &path);

    geometry_msgs::msg::TwistStamped computeVelocityCommands(
            const geometry_msgs::msg::PoseStamped &pose, geometry_msgs::msg::Twist &velocity);

    void setPlan(const nav_msgs::msg::Path &path);

    bool Finished() { return global_path_remain.poses.empty(); };

    bool FinishedVirtualWall(double radius);

    bool FinishedVirtualWall();

    geometry_msgs::msg::PoseStamped goalPose(bool global_frame = true, bool earse_this = false);

    bool setNextGoal(geometry_msgs::msg::PoseStamped &pose_, double distance_sqr);

protected:
    nav_msgs::msg::Path transformGlobalPlan(const nav_msgs::msg::Path &path);

    bool transformPose(const std::shared_ptr<tf2_ros::Buffer> tf, const std::string frame,
                       const geometry_msgs::msg::PoseStamped &in_pose,
                       geometry_msgs::msg::PoseStamped &out_pose, rclcpp::Duration &transform_tolerance);

    rclcpp::Node::SharedPtr node_;
    std::shared_ptr<tf2_ros::Buffer> tf_;
    rclcpp::Clock::SharedPtr clock_;
    geometry_msgs::msg::PoseStamped goal_pose, end_pose;
    nav_msgs::msg::Path global_path_original, global_path_remain;
    nav_msgs::msg::Path plan_in_robot_frame;
    rclcpp::Duration transform_tolerance_{0, 0};
    double goal_tolerate_dis;
    double goal_tolerate_angle;
    double desired_linear_vel_;
    double lookahead_dist_;
    double max_angular_vel_;
    double min_angular_vel_;
    double last_dis_p = 0;
    double total_err = 0;
    double distance_ = 0;
    int method = 1;
    bool last_turn_finished = false;
    size_t truncation_num = 20;
};


#endif 